#include <p32xxxx.h>
#include <plib.h>

#include "interpolate.h"
#include "quantize96.h"

#define AUDIO_BUFF_SIZE 64 /* MUST BE a power of two! */
#define AUDIO_BUFF_HALF (AUDIO_BUFF_SIZE/2)
#define AUDIO_SAMPLING_FREQUENCY 96000
#define AUDIO_PWM_OFFSET 208 /* pulse width offset */
#define AUDIO_PWM_SCALE 3
#define AUDIO_PWM_SHIFT 9

static int16_t audio_buffer[AUDIO_BUFF_SIZE];
static volatile short audio_play_pos;
static short audio_write_pos;
static struct quantize96_state audio_qstate;
static struct twice_state audio_istate;

/**
 * timer interrupt which sets the pulse width of a new pulse for
 * audio PWM according to a value that is stored in audio_buffer
 */
void audio_pulse_interrupt(void)
{
	short pos, smp;
	pos = audio_play_pos;
	smp = audio_buffer[pos];
	/* TODO: set AUDIO_PWM_OFFSET+smp as duty cycle for the next pulse */
	audio_buffer[pos] = AUDIO_PWM_OFFSET; /* silence */
	audio_play_pos = (pos+1) & (AUDIO_BUFF_SIZE-1);
}

/**
 * blocking write to the audio buffer
 * @param len
 *   number of samples to process
 * @param in
 *   pointer to input samples
 * @param half_fs_flag
 *   set this parameter to a nonzero value if your samples are from
 *   a PCM stream of half the sampling rate. It will then be interpolated.
 */
void audio_blocking_write(int len, const int16_t in[], int half_fs_flag)
{
	short chunk; /* block of samples to move in one go */
	if (half_fs_flag) {
		audio_write_pos &= ~1u;
		len <<= 1;
	}
	while (len>0) {
		if ((audio_write_pos & (AUDIO_BUFF_HALF-1))==0) {
			/* we're at the start of a new half. let's wait for
			 * the playback to finish it before we write new
			 * data into it ... */
#if 0
			while (((audio_play_pos^audio_write_pos)&AUDIO_BUFF_HALF)==0);
#endif
		}
		/* how many samples can we write until we hit the boundary
		 * to the next half? */
		chunk = AUDIO_BUFF_HALF-(audio_write_pos&(AUDIO_BUFF_HALF-1));
		/* reduce chunk to len if we don't have enough data to fill
		 * this half ... */
		if (len<chunk) chunk=len;
		/* turn 16 bit samples into pulse widths ... */
		if (half_fs_flag) {
			twice_process(&audio_istate,chunk>>1,in,audio_buffer+audio_write_pos);
			in += chunk>>1;
			quantize96_process(&audio_qstate,
				AUDIO_PWM_SCALE,AUDIO_PWM_SHIFT,
				chunk,audio_buffer+audio_write_pos,audio_buffer+audio_write_pos);
		} else {
			quantize96_process(&audio_qstate,
				AUDIO_PWM_SCALE,AUDIO_PWM_SHIFT,
				chunk,in,audio_buffer+audio_write_pos);
			in  += chunk;
		}
		len -= chunk;
		audio_write_pos = (audio_write_pos + chunk) & (AUDIO_BUFF_SIZE-1);
	}
}

void picperftest(void)
{
	#define DAT_SIZ 48
	int pass;
	int16_t data[DAT_SIZ] = {0};
	while (1) {
		for (pass=0; pass<500; ++pass) {
			audio_blocking_write(DAT_SIZ,data,1);
		}
		mPORTAToggleBits(BIT_10);
	}
}

int main(void)
{
	/* Configure PB frequency and wait states */
	SYSTEMConfigPerformance(40000000L);
	/* Configure LED pin (Pinguino D9/LED2, MCU pin 12: RA10) */
	mPORTASetPinsDigitalOut(BIT_10);
	picperftest();
	return 0;
}
